package dk.controlme.lego.tester;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;

import dk.controlme.lego.common.Action;
import lejos.pc.comm.*;

public class Model {
    private DataOutputStream dataOutputStream;
    private DataInputStream dataInputStream;
    private NXTConnector nxtConnector;
    private boolean connected;
    private Action lastActionSent;

    public Model() {
        connected = false;
    }

    public boolean isConnected() {
        return connected;
    }

    public boolean connect(String name) {
        NXTComm nxtComm = null;
        try{
            // Create NXT Communicator
            nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
            // Get info on requested NXT using bluetooth
            //TODO[jaro]: Add nice handling of "no NXT of that name found" type errors.
            NXTInfo[] nxtInfo = nxtComm.search(name, NXTCommFactory.BLUETOOTH);
            // Open the first object -- assume we only have 1 NXT connected.
            //TODO[jaro]: Add nice handling of several NXTs with same name..
            connected = nxtComm.open(nxtInfo[0]);
        } catch(NXTCommException e) {
            System.out.println("Unable to create NXT Communicator object: "+e.getMessage());
            e.printStackTrace();
            System.exit(-1);
        }
        if(nxtComm != null) {
            dataInputStream = new DataInputStream(nxtComm.getInputStream());
            dataOutputStream = new DataOutputStream(nxtComm.getOutputStream());
        }
        return isConnected();
    }

    public void sendAction(Action action) {
        // No reason to send same command as currently being executed
        if (action != lastActionSent) {
            try {
                int actionId = action.getId();
                System.out.println("Sending ["+actionId+"] "+action);
                dataOutputStream.writeInt(actionId);
                dataOutputStream.flush();
            } catch(IOException e) {
                System.out.println("IO Exception writing bytes:");
                System.out.println(e.getMessage());
            }
            lastActionSent = action;
        }
    }

    public void closeConnection() {
        // Stop robot before exiting
        if(isConnected()) sendAction(Action.STOP);
        // Cleanup
        try {
            dataOutputStream.close();
            dataInputStream.close();
            nxtConnector.close();
        } catch(IOException e) {
            System.out.println("IO Exception closing connection:");
            System.out.println(e.getMessage());
        }
    }

}
